/*! ServoTest 
	test program for controlling servo

	author: kyon
*/
#include "ros/ros.h"
#include "camera/servo.h"

using namespace camera;
ros::Publisher servo_pub;

int main(int ac, char **av) {
	ros::init(ac, av, "servoTest");
	ros::NodeHandle n;

	servo_pub = n.advertise<servo>("/serial/move_servo", 10000);

	int cnt = 15;
	ros::Rate loop_rate(1);
	while(ros::ok()) {
		servo sv;
		sv.id = 4;
		cnt *= -1;
		sv.angle = 30+cnt;
		ROS_INFO("Set servo pos: %d", sv.angle);
		servo_pub.publish(sv);
		ros::spinOnce();
		loop_rate.sleep();
	}
	return 0;
}
